/*
接收JY901姿态传感器的数据，并转化为角度制赋给全局变量
*/

#include <Arduino.h>
#include "GET_IMU.h"
#include "CONTROL.h"

float JY901Pitch,JY901Yaw,JY901Wy;
short stcAngle[2],stcGyro;


void JY901() 
{
    //配合JY901的数据手册一起读还是很好理解的  "JY901\操作说明书\JY901使用说明书V4.4.pdf"
    //JY901的通讯协议还是很简单的 |起始位|标志位|数据数据数据|当前温度|校验位| 
    static unsigned char ucRxBuffer[12];
    static unsigned char ucRxCnt = 0; 
    ucRxBuffer[ucRxCnt++]=Serial2.read();
    if (ucRxBuffer[0]!=0x55)    //0x55是JY901数据的起始位
    {
        ucRxCnt=0;
        return;
    }
    if (ucRxCnt<11) {return;}   //没有收完整个数据帧就返回等待后续数据
    else
    {
        switch(ucRxBuffer[1])
        {
        case(0x53):memcpy(&stcAngle,&ucRxBuffer[4],4);break;    //标志位为0x53的数据帧包含当前角度的欧拉角
        case(0x52):memcpy(&stcGyro,&ucRxBuffer[4],2);break;     //标志位为0x52的数据帧包含当前绕各轴旋转的角速度
        //memcpy(&目标地址,&指定地址,x)
        //memcpy函数是从指定地址开始，向另一个地址搬运x位数据，注意不同数据类型占的位数
        }
        ucRxCnt=0; 
    }
}
void GET_Angle()
{   
    //根据JY901数据手册 "JY901\操作说明书\JY901使用说明书V4.4.pdf"第40页角度计算公式进行转换
    float pitch;
    pitch=(float)stcAngle[0]/32768*180;

    if ( pitch>40||pitch<-40 ){return;}   //带通滤波（其实就是把过大过小的数据剔除，叫带通滤波显得高大上一点）
    else{JY901Pitch=pitch;}

    JY901Yaw=(float)stcAngle[1]/32768*180;
    JY901Wy=(float)stcGyro/32768*2000;

    
}